#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <cfloat>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <functional> // std::bind
#include <iostream>
#include <iostream> // std::cout
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
using namespace std;

template <class T> struct Point {
  T x, y, z, yaw, s;

  Point() : x(0), y(0), z(0), yaw(0), s(0) {}

  Point(T x, T y, T z, T yaw, T s) : x(x), y(y), z(z), yaw(yaw), s(s) {}

  Point(T x, T y) : x(x), y(y), z(0), yaw(0), s(0) {}
  Point(T x, T y, T s) : x(x), y(y), z(0), yaw(0), s(s) {}
};

template <typename T>
/**
 * @brief Calculate the distance between two points.
 * @param x1 The first point's x.
 * @param y1 The first point's y.
 * @param x2 The second point's x.
 * @param y2 The second point's y.
 */
T Distance(const T &x1, const T &y1, const T &x2, const T &y2) {
  return std::hypot(x1 - x2, y1 - y2);
}

template <typename T>
/**
 * @brief Calculate the distance square between two points.
 * @param x1 The first point's x.
 * @param y1 The first point's y.
 * @param x2 The second point's x.
 * @param y2 The second point's y.
 */
T DistanceSquare(const T &x1, const T &y1, const T &x2, const T &y2) {
  return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
}

template <typename T>
T CrossProduct(const T x0, const T y0, const T x1, const T y1) {
  return x0 * y1 - x1 * y0;
}

template <typename T>
T InnerProduct(const T x0, const T y0, const T x1, const T y1) {
  return x0 * x1 + y0 * y1;
}

template <typename T>
T PointLineDistanceSquare(const T s_x, const T s_y, const T e_x, const T e_y,
                          const T x, const T y) {
  T line_length = Distance(s_x, s_y, e_x, e_y);
  if (line_length < 1e-6) {
    return DistanceSquare(s_x, s_y, x, y);
  }
  T proj = InnerProduct(x - s_x, y - s_y, e_x - s_x, e_y - s_y) / line_length;
  if (proj <= 0.0) {
    return DistanceSquare(s_x, s_y, x, y);
  }
  if (proj >= line_length) {
    return DistanceSquare(e_x, e_y, x, y);
  }
  T distance =
      CrossProduct(x - s_x, y - s_y, e_x - s_x, e_y - s_y) / line_length;
  return distance * distance;
}

#define M_2PI 6.28318530717958647692
// deprecated, use UnifyTheta instead
template <typename T> T unify_theta(T theta) {
  while (theta > M_PI)
    theta -= M_2PI;
  while (theta < -M_PI)
    theta += M_2PI;
  return theta;
}

template <typename T>
void GetIntervalIndexByXY(const std::vector<T> &points, const float x,
                          const float y, const float *yaw, int *const i_1st,
                          int *const i_2nd, float *const weight = NULL,
                          int *const i_start = NULL) {
  if (points.size() < 2)
    return;
  int min_i = (i_start ? *i_start : 0);
  min_i = std::max(min_i, 0);
  min_i = std::min(min_i, static_cast<int>(points.size() - 2));
  float min_distance =
      PointLineDistanceSquare(points[min_i].x, points[min_i].y,
                              points[min_i + 1].x, points[min_i + 1].y, x, y);
  for (int i = min_i + 1; i < (points.size() - 1); ++i) {
    float tmp_distance = PointLineDistanceSquare(
        points[i].x, points[i].y, points[i + 1].x, points[i + 1].y, x, y);
    if (tmp_distance < min_distance &&
        (yaw == NULL ||
         (yaw != NULL &&
          fabs(unify_theta(*yaw - points.at(i).yaw)) < 0.25 * M_PI))) {
      min_distance = tmp_distance;
      min_i = i;
    }
  }
  std::cout << "min_i:" << min_i << std::endl;
  *i_1st = min_i;
  *i_2nd = *i_1st + 1;
  if (i_start != NULL) {
    *i_start = *i_1st;
  }
  if (weight != NULL) {
    float theta = std::atan2(points.at(*i_2nd).y - points.at(*i_1st).y,
                             points.at(*i_2nd).x - points.at(*i_1st).x);
    float dx = x - points.at(*i_1st).x, dy = y - points.at(*i_1st).y;
    float projection = dx * std::cos(theta) + dy * std::sin(theta);
    float length = Distance(points.at(*i_1st).x, points.at(*i_1st).y,
                            points.at(*i_2nd).x, points.at(*i_2nd).y);
    *weight = length < 0.00001f ? 0.0f : projection / length;
    if (*i_1st != 0) {
      *weight = std::max(*weight, 0.0f);
    }
    if (*i_2nd + 1 != points.size()) {
      *weight = std::min(*weight, 1.0f);
    }
  }
}

/**
 * @brief Calculate the weighted average value of two numbers.
 * @param n1 The first number.
 * @param n2 The second number.
 * @param weight The value of (result - n1)/(n2 - n1).
 */
template <typename T>
inline T LinearInterpolationNumber(const T &n1, const T &n2,
                                   const float &weight) {
  return (1.0f - weight) * n1 + weight * n2;
}

/**
 * @brief Calculate the weighted average value of all of member variables of
 * two planning points.
 * @param p1 The first point.
 * @param p2 The second point.
 * @param weight The value of (result - n1)/(n2 - n1).
 * @return The weighted average result.
 */
template <typename T>
Point<T> LinearInterpolationPoint(const Point<T> &p1, const Point<T> &p2,
                                  const float weight) {
  Point<T> result;
  result.x = LinearInterpolationNumber(p1.x, p2.x, weight);
  result.y = LinearInterpolationNumber(p1.y, p2.y, weight);
  result.s = LinearInterpolationNumber(p1.s, p2.s, weight);
  return result;
}

void CartesianToFrenet(const float rx, const float ry, const float rtheta,
                       const float x, const float y, float *const ptr_l) {
  const float dx = x - rx;
  const float dy = y - ry;
  const float cross_rd_nd = std::cos(rtheta) * dy - std::sin(rtheta) * dx;
  *ptr_l = std::copysign(hypot(dx, dy), cross_rd_nd);
}

template <typename T>
bool XYToSL(const float x, const float y, const float *yaw, float *const s,
            float *const l, std::vector<T> &points,
            int *const i_start = nullptr,
            Point<float> *const norm_vec = nullptr) {
  if (points.size() <= 1) {
    return false;
  }
  int i_1st, i_2nd;
  float weight;
  GetIntervalIndexByXY(points, x, y, yaw, &i_1st, &i_2nd, &weight, i_start);
  std::cout << "i_1st:" << i_1st << " i_2nd:" << i_2nd << std::endl;
  auto ref_p =
      LinearInterpolationPoint(points.at(i_1st), points.at(i_2nd), weight);
  *s = ref_p.s;
  std::cout << "ref_p.s:" << ref_p.s << std::endl;
  CartesianToFrenet(ref_p.x, ref_p.y, ref_p.yaw, x, y, l);

  Point<float> ref_line, cur_p;
  if (i_1st != i_2nd) {
    ref_line = Point<float>(points.at(i_2nd).x - points.at(i_1st).x,
                            points.at(i_2nd).y - points.at(i_1st).y),
    cur_p = Point<float>(x - points.at(i_1st).x, y - points.at(i_1st).y);
  } else {
    if (i_1st != 0) {
      ref_line = Point<float>(points.at(i_1st).x - points.at(i_1st - 1).x,
                              points.at(i_1st).y - points.at(i_1st - 1).y),
      cur_p =
          Point<float>(x - points.at(i_1st - 1).x, y - points.at(i_1st - 1).y);
    } else {
      ref_line = Point<float>(points.at(i_1st + 1).x - points.at(i_1st).x,
                              points.at(i_1st + 1).y - points.at(i_1st).y),
      cur_p = Point<float>(x - points.at(i_1st).x, y - points.at(i_1st).y);
    }
  }
  float prod = ref_line.x * cur_p.y - ref_line.y * cur_p.x;
  *l = std::copysign(*l, prod);

  if (norm_vec != nullptr) {
    norm_vec->y = std::copysign(ref_line.x, prod);
    norm_vec->x = norm_vec->y * ref_line.x >= 0.0f ? -ref_line.y : ref_line.y;
    float value = std::fmax(1e-5f, std::hypot(norm_vec->x, norm_vec->y));
    norm_vec->x /= value;
    norm_vec->y /= value;
  }

  return true;
}



int main() {
  //   cout << "Hello World" << endl;
  //   float a = 15.079011f;
  //   float b = -4.278343f;
  //   int aa = (round(a + 500)) * 1000000;
  //   int bb = round(b + 500);
  //   int res = (round(a + 500)) * 1000000 + round(b + 500);
  //   int new_res = aa + bb;
  //   cout << aa << endl;
  //   cout << bb << endl;
  //   cout << res << endl;
  //   cout << new_res << endl;
  std::cout << "=====================>" << std::endl;
  std::vector<Point<float>> reference_line;
  reference_line.emplace_back(-10.0f, 1.0f, 0.0f);
  reference_line.emplace_back(-8.1f, 0.9f, 2.0f);
  reference_line.emplace_back(-6.0f, 0.8f, 4.3f);
  reference_line.emplace_back(-5.1f, 0.5f, 5.1f);
  reference_line.emplace_back(-2.1f, 0.3f, 8.1f);
  reference_line.emplace_back(-0.1f, 0.1f, 9.9f);
  reference_line.emplace_back(2.1f, 0.1f, 12.1f);
  reference_line.emplace_back(5.0f, 0.0f, 15.0f);
  reference_line.emplace_back(7.1f, -0.1f, 17.1f);
  float car_s = 0.0f;
  float car_l = 0.0f;
  XYToSL(0.0f, 0.0f, NULL, &car_s, &car_l, reference_line);
  std::cout << "car_s:" << car_s << " car_l:" << car_l << std::endl;
  
  return 0;
}